#!/usr/bin/env python

import sys
import math
import rospy
from sensor_msgs.msg import Image
from vision_messages.srv import ObjPoseEst
from geometry_msgs.msg import Point
from visualization_msgs.msg import Marker, MarkerArray
import numpy as np
import cv2
import message_filters

class Perception_Client():
    def __init__(self):
        # rospy.init_node('percenption_client')

        # add_thread = threading.Thread(target = thread_job)
        # add_thread.start()
        self.get_image()

    def client(self):
        rospy.wait_for_service('/perception/vision_perception')
        try:
            get_pose = rospy.ServiceProxy('/perception/vision_perception', ObjPoseEst)
            result = get_pose(self.image, self.depth)
            return result
        except rospy.ServiceException as e:            
            rospy.loginfo("Perception Service call failed: %s"%e)

    def get_image(self):
        image_sub = message_filters.Subscriber('/camera/color/image_raw', Image)
        depth_sub = message_filters.Subscriber('/camera/aligned_depth_to_color/image_raw', Image)
        ts = message_filters.TimeSynchronizer([image_sub, depth_sub], 10)
        ts.registerCallback(self.callback)
        
        # rospy.Subscriber("/camera/color/image_raw", Image, self.image_callback)
        self.image = rospy.wait_for_message("/camera/color/image_raw", Image)

        # rospy.Subscriber("/camera/aligned_depth_to_color/image_raw", Image, self.depth_callback)
        self.depth = rospy.wait_for_message("/camera/aligned_depth_to_color/image_raw", Image)

        # print(self.image.header.stamp)
        # print(self.depth.header.stamp)

    def callback(self, image, depth):
        # rospy.loginfo('Get Image and Depth!')
        # self.image = image
        # self.depth = depth
        pass
        # print(self.image.header.stamp)
        # print(self.depth.header.stamp)

    def image_callback(self, data):
        self.image = data

    def depth_callback(self, data):
        self.depth = data

if __name__ == "__main__":
    while True:
        rospy.init_node('percenption_client_debug')
        percenption = Perception_Client()
        results = percenption.client()
        if results is not None:
            for i in range(len(results.obj_pose_list)):
                result = results.obj_pose_list[i]
                obj_pose = result.RT
                obj_cate = result.category_name
                obj_scales = result.scales
                obj_scale = result.scale
                rospy.loginfo('Get Obj Cate: \t \t{}'.format(obj_cate))
                rospy.loginfo('Get Obj Pose: \n \t{}'.format(np.array(obj_pose.data).reshape([4, 4])))
                rospy.loginfo('Get Obj Scale: \n \t{}'.format(obj_scale))
                rospy.loginfo('Get Obj Scales: \n \t{}'.format(np.array(obj_scales)))
        else:
            rospy.loginfo('WARNING! No Object detected! Please check again!')
        # break
